#ifndef OMNI_DRIVE_H
#define OMNI_DRIVE_H

#include "motor_controller.h"

// 四轮万向轮全向驱动实现
class OmniDrive : public MotorController {
private:
  int motorFL1, motorFL2, motorFR1, motorFR2;
  int motorBL1, motorBL2, motorBR1, motorBR2;
  int maxSpeed;

public:
  OmniDrive(int fl1, int fl2, int fr1, int fr2, int bl1, int bl2, int br1, int br2) {
    motorFL1 = fl1;
    motorFL2 = fl2;
    motorFR1 = fr1;
    motorFR2 = fr2;
    motorBL1 = bl1;
    motorBL2 = bl2;
    motorBR1 = br1;
    motorBR2 = br2;
    maxSpeed = 255;
  }

  void init() override {
    pinMode(motorFL1, OUTPUT);
    pinMode(motorFL2, OUTPUT);
    pinMode(motorFR1, OUTPUT);
    pinMode(motorFR2, OUTPUT);
    pinMode(motorBL1, OUTPUT);
    pinMode(motorBL2, OUTPUT);
    pinMode(motorBR1, OUTPUT);
    pinMode(motorBR2, OUTPUT);
    stop();
  }

  void forward(int speed) override {
    speed = constrain(speed, 0, maxSpeed);
    // 所有电机正转
    analogWrite(motorFL1, speed);
    analogWrite(motorFL2, 0);
    analogWrite(motorFR1, speed);
    analogWrite(motorFR2, 0);
    analogWrite(motorBL1, speed);
    analogWrite(motorBL2, 0);
    analogWrite(motorBR1, speed);
    analogWrite(motorBR2, 0);
  }

  void backward(int speed) override {
    speed = constrain(speed, 0, maxSpeed);
    // 所有电机反转
    analogWrite(motorFL1, 0);
    analogWrite(motorFL2, speed);
    analogWrite(motorFR1, 0);
    analogWrite(motorFR2, speed);
    analogWrite(motorBL1, 0);
    analogWrite(motorBL2, speed);
    analogWrite(motorBR1, 0);
    analogWrite(motorBR2, speed);
  }

  void turnLeft(int speed) override {
    speed = constrain(speed, 0, maxSpeed);
    // 左侧电机反转，右侧电机正转
    analogWrite(motorFL1, 0);
    analogWrite(motorFL2, speed);
    analogWrite(motorFR1, speed);
    analogWrite(motorFR2, 0);
    analogWrite(motorBL1, 0);
    analogWrite(motorBL2, speed);
    analogWrite(motorBR1, speed);
    analogWrite(motorBR2, 0);
  }

  void turnRight(int speed) override {
    speed = constrain(speed, 0, maxSpeed);
    // 左侧电机正转，右侧电机反转
    analogWrite(motorFL1, speed);
    analogWrite(motorFL2, 0);
    analogWrite(motorFR1, 0);
    analogWrite(motorFR2, speed);
    analogWrite(motorBL1, speed);
    analogWrite(motorBL2, 0);
    analogWrite(motorBR1, 0);
    analogWrite(motorBR2, speed);
  }

  void stop() override {
    analogWrite(motorFL1, 0);
    analogWrite(motorFL2, 0);
    analogWrite(motorFR1, 0);
    analogWrite(motorFR2, 0);
    analogWrite(motorBL1, 0);
    analogWrite(motorBL2, 0);
    analogWrite(motorBR1, 0);
    analogWrite(motorBR2, 0);
  }

  void setSpeed(int speed) override {
    maxSpeed = constrain(speed, 0, 255);
  }

  String getType() override {
    return "OmniDrive";
  }
};

#endif // OMNI_DRIVE_H